// Copyright 2012 Sergio, Raul, Dorival. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#ifndef GEMLAB_LINALG_H
#define GEMLAB_LINALG_H

//#define EIGEN_DONT_VECTORIZE
//#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
//#define EIGEN_DONT_ALIGN

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace GemLab {

typedef Eigen::Matrix<double,3,1>                           Vec3_t;
typedef Eigen::Matrix<double,3,3>                           Mat3_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1>              Vec_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> Mat_t;

// TODO: Fix this
// THE FOLLOWING METHODS ARE AMBIGUOUS BECAUSE Vec3_t ~ Vec_t
/*
inline Vec3_t Cross(Vec3_t const & P0, Vec3_t const & P1)
{
    return P0.cross(P1);
}

inline double Dot(Vec3_t const & P0, Vec3_t const & P1)
{
    return P0.dot(P1);
}

inline double Norm(Vec3_t const & P0)
{
    return P0.norm();
}

inline bool Solve(Mat3_t const & M, Vec3_t const & B, Vec3_t & X, double Tol=1.0e-10)
{
    // determinant
    double det =   M(0,0)*(M(1,1)*M(2,2) - M(1,2)*M(2,1))
                 - M(0,1)*(M(1,0)*M(2,2) - M(1,2)*M(2,0))
                 + M(0,2)*(M(1,0)*M(2,1) - M(1,1)*M(2,0));
    if (fabs(det)<Tol) return false;
    
    // inverse matrix
    Mat3_t Mi;
    Mi(0,0) = (M(1,1)*M(2,2) - M(1,2)*M(2,1)) / det;
    Mi(0,1) = (M(0,2)*M(2,1) - M(0,1)*M(2,2)) / det;
    Mi(0,2) = (M(0,1)*M(1,2) - M(0,2)*M(1,1)) / det;

    Mi(1,0) = (M(1,2)*M(2,0) - M(1,0)*M(2,2)) / det;
    Mi(1,1) = (M(0,0)*M(2,2) - M(0,2)*M(2,0)) / det;
    Mi(1,2) = (M(0,2)*M(1,0) - M(0,0)*M(1,2)) / det;

    Mi(2,0) = (M(1,0)*M(2,1) - M(1,1)*M(2,0)) / det;
    Mi(2,1) = (M(0,1)*M(2,0) - M(0,0)*M(2,1)) / det;
    Mi(2,2) = (M(0,0)*M(1,1) - M(0,1)*M(1,0)) / det;

    // solve system
    X(0) = Mi(0,0)*B(0) + Mi(0,1)*B(1) + Mi(0,2)*B(2);
    X(1) = Mi(1,0)*B(0) + Mi(1,1)*B(1) + Mi(1,2)*B(2);
    X(2) = Mi(2,0)*B(0) + Mi(2,1)*B(1) + Mi(2,2)*B(2);

    return true;
}
*/

/*
inline Vec_t Solve(Mat_t const & A, Vec_t const & b)
{
    return A.colPivHouseholderQr().solve(b);
}
*/

}; // namespace GemLab

#endif // GEMLAB_LINALG_H
